import pickle
from math import cos, sin

import numpy as np
import os

import FrameTrans

pks = [
	'1692604696041.pickle',
	'1692604698042.pickle',
	'1692604700041.pickle'
]

lds = []


class Integration:
	reference = []
	basepose = []
	pointcloud = []

	def init(self, reference, pose):
		self.reference = reference
		self.basepose = pose

	def integrate(self, reference, rpy, lidardata):
		dX = self.reference[0] - reference[0]
		dY = self.reference[1] - reference[1]
		dZ = self.reference[2] - reference[2]

		rx = self.basepose[0] - rpy[0]
		ry = self.basepose[1] - rpy[1]
		rz = self.basepose[2] - rpy[2]

		R1 = np.mat([
			[1, 0, 0],
			[0, cos(rx), sin(rx)],
			[0, -sin(rx), cos(rx)]
		])

		R2 = np.mat([
			[cos(ry), 0, -sin(ry)],
			[0, 1, 0],
			[sin(ry), 0, cos(ry)]
		])

		R3 = np.mat([
			[cos(rz), sin(rz), 0],
			[-sin(rz), cos(rz), 0],
			[0, 0, 1]
		])

		for ld in lidardata:
			x, y, z, i = ld

			pos = np.mat([x, y, z]).transpose()
			bias = np.mat([dX, dY, dZ]).transpose()
			new_pos = R3*R2*R1*pos + bias
			self.pointcloud.append([new_pos[0, 0], new_pos[1, 0], new_pos[2, 0], i])


integration = Integration()
position = FrameTrans.Position()

for pk in pks:
	with open(pk, 'rb') as f:
		data = pickle.load(f)
		b = data['gps_data']['lat']
		l = data['gps_data']['lon']
		h = data['gps_data']['height']
		y = data['gps_data']['yaw'] * FrameTrans.D2R
		p = data['gps_data']['pitch'] * FrameTrans.D2R
		r = data['gps_data']['roll'] * FrameTrans.D2R
		lidar_data = data['forward_lidar_data']['data']
		lds.append(lidar_data)

		rpy = (r, p, y)
		# 这里不用转FLU好像效果更好，转FLU反而歪了
		# rpy = position.FRD2FLU(r, p, y)

		if not integration.reference:
			position.init(b, l, h)
			integration.init(position.BLH2XYZ(b, l, h), rpy)

		xyz = position.BLH2XYZ(b, l, h)
		integration.integrate(xyz, rpy, lidar_data)

xs = []
ys = []
zs = []
ints = []
for pt in integration.pointcloud:
	xs.append(pt[0])
	ys.append(pt[1])
	zs.append(pt[2])
	ints.append(pt[3])

savefilename = "integrated.pcd"
if not os.path.exists(savefilename):
	f = open(savefilename, 'w')
	f.close()
with open(savefilename, 'w') as file_to_write:
	file_to_write.writelines("# .PCD v0.7 - Point Cloud Data file format\n")
	file_to_write.writelines("VERSION 0.7\n")
	file_to_write.writelines("FIELDS x y z i\n")
	file_to_write.writelines("SIZE 4 4 4 4\n")
	file_to_write.writelines("TYPE F F F F\n")
	file_to_write.writelines("COUNT 1 1 1 1\n")
	file_to_write.writelines("WIDTH " + str(len(xs)) + "\n")
	file_to_write.writelines("HEIGHT 1\n")
	file_to_write.writelines("VIEWPOINT 0 0 0 1 0 0 0\n")
	file_to_write.writelines("POINTS " + str(len(xs)) + "\n")
	file_to_write.writelines("DATA ascii\n")
	for i in range(len(xs)):
		file_to_write.writelines(str(xs[i]) + " " + str(ys[i]) + " " + str(zs[i]) + " " + str(ints[i]) + "\n")
